#ifndef MODULES_ROUTING_TOPOMAP_STRATEGY_H_
#define MODULES_ROUTING_TOPOMAP_STRATEGY_H_

#include "ros/ros.h"
#include <vector>
#include <unordered_map>
#include <cmath>
#include <string>
#include <algorithm>
#include <iostream>
#include <nav_msgs/Path.h>
#include <typeinfo>
#include <fstream>
#include <json/json.h>

namespace CommonLib
{
    class PointAttr;
    class PathAttr;
    class TopoGraph;

    class PointAttr // used to parse data type waypoints in  yaml file
    {
    public:
        PointAttr() { x = y = z = 0.0; }
        PointAttr(Json::Value value);
        ~PointAttr() {}
        std::string pointid;
        double x, y, z;
    };

    class PathAttr
    {
    public:
        PathAttr() {controlpoints.clear(); keypoints.clear();};
        PathAttr(Json::Value value);
        ~PathAttr() {}

        std::string pathid;
        std::vector<std::string> controlpoints;
        std::vector<std::string> keypoints;
        std::string start;
        std::string end;
        
        std::string direction;

        double maxspeed;  // 地图设置最大速度
        double amr_angle;  // 车体角度
        std::string angle_type;
        std::string pathtype;
    };

    class TopoGraph
    {
    public:
        TopoGraph()  = default;
        ~TopoGraph() = default;

        bool LoadGraph(std::string mapfile);
        std::unordered_map<std::string, std::shared_ptr<PointAttr>> &GetPointMap() { return ptr_point; }
        std::unordered_map<std::string, std::shared_ptr<PathAttr>> &GetPathMap() { return ptr_path; }
    
    private:
        std::unordered_map<std::string, std::shared_ptr<PointAttr>> ptr_point;
        std::unordered_map<std::string, std::shared_ptr<PathAttr>> ptr_path;
        
        bool LoadPoints(Json::Value &value);
        bool LoadPaths(Json::Value &value);

        std::string timestamp;
    };
}
#endif